#pragma once
#include "Config.h"
#include <spline.h>

struct LineEq {
	CVector<float> C;
	float zStart;
	float zEnd;

	/**@warning points value's z-order: must be strictly monotone increasing*/
	mutable CVector<P> points;
	mutable tk::spline spline;
	mutable bool isPointsInterpolated = false;

	enum Side { None, Left, Right, Tripple_Left, Tripple_Right };
	Side side = Side::None;

	bool isValid() const { return C.size() == 4 || points.size() > 2; }

	bool containZ(float z, float epsilon = 1e-6) const { return z > zEnd - epsilon && z < zStart + epsilon; }

	bool isPointsData() const { return !points.empty(); }
	//bool operator==(const LineEq& other) const { return C == other.C && zStart == other.zStart && zEnd == other.zEnd; }
	//bool operator!=(const LineEq& other) const { return !(*this == other); }

	/**@note `set_points(a,b,type)` first param must be strictly monotone increasing*/
	void getInterpolatedValue(float z, float* x, float* dx, tk::spline::spline_type splineType = tk::spline::linear) const {
		if (!isPointsInterpolated) {
			isPointsInterpolated = true;

			std::vector<double> Z, X;
			for (const auto& p : points) {
				Z.push_back(p.z);
				X.push_back(p.x);
			}
			spline.set_points(Z, X, splineType);// param-1 must be increasing
		}
		if (x != nullptr)
			*x = spline(z) + sideOffset(side);
		if (dx != nullptr)
			*dx = 1 / spline.deriv(1, z);
	}

	LineEq sideEq(Side side) const {
		if (Side::None == side)
			return *this;
		LineEq eq;
		if (!isPointsData()) {
			eq.C = this->C;
			eq.zStart = this->zStart;
			eq.zEnd = this->zEnd;
			float deltaC0 = -sideOffset(side);// left direction is positive
			eq.C[0] += deltaC0;
			return eq;
		}
		eq.C = this->C;
		eq.zStart = this->zStart;
		eq.zEnd = this->zEnd;
		eq.spline = this->spline;
		eq.isPointsInterpolated = this->isPointsInterpolated;
		if (!eq.isPointsInterpolated)
			eq.points = this->points;
		eq.side = side;
		return eq;
	}

	float sideOffset(Side side) const {// left negative, right positive
		if (side == Side::None)
			return 0;
		const auto HInterval = Config::Line_HInterval;
		float deltaX = 0;
		if (side == Side::Left || side == Side::Right)// double line
			deltaX = Config::Line_Width / 2 + HInterval / 2;
		else if (side == Side::Tripple_Left || side == Side::Tripple_Right)// tripple line
			deltaX = Config::Line_Width + HInterval;
		deltaX *= ((side == Side::Left || side == Side::Tripple_Left) ? -1 : 1);
		return deltaX;
	}
};

/***********************************************************************/
/***********************************************************************/
struct LineArg {
	LineTag tag;
	LineEq eq;
	LineStyle style;
	CColor color = Config::Line_DefColor;
	bool isCrossing = false;// is crossing lane line
	int probability = 100;// Target probability: 0 ~ 100
	bool identified = true;
	int updateCnt = -1;
	int updateFrameCnt = -1;

	bool isPointsData() const { return eq.isPointsData(); }

	bool isValid() const { return updateCnt != -1; }
};

/** @warning default boolean value should be set, otherwise, a random value may be setted! */
struct ModelArg {
	int ID;
	ModelType type;
	P pos;
	float angle = 0;// Unit: radians
	bool isOncoming = false;
	bool isAccTracking = false;
	CColor color;
	MoveStatus moveStatus;
	int probability = 100;// Target probability: 0 ~ 100
	float width = 0;// Unit: m
	float length = 0;// Unit: m
	Vec2 relSpeed = Vec2(0);// Relatively close:negative, relatively far away:positive;Unit: m/s
	bool identified = true;

	int updateCnt = -1;
	int updateFrameCnt = -1;

	bool isValid() const { return updateCnt != -1; }

	int recog = -1;// SSQK-Project just for temp usage!
};

struct ZhicheArg {
	P pos = P(0);
	float angle = 0;// Unit: radians
	float speed = 0;// Unit: km/h
	float width = 0;// Unit: m
	float length = 0;// Unit: m

	bool isValid() const { return width > 0 && length > 0; }
};

/** @note Status: not be associated with LineArg and ModleArg directly*/
struct Status {
	DCLC dclc = DCLC::INVALID;//Config::TestCase_HideDclcArrow ? DCLC::None : DCLC::Mid;
	ALC alc = ALC::INVALID;
	LDW ldw = LDW::INVALID;
	ACC_TG accTG = ACC_TG::INVALID;
	TailLamp tailLamp = TailLamp::INVALID;
};

/*******************Project SQ*************************/
/******************************************************/
/******************************************************/
namespace sq {
	struct ModelArg {
		int id;
		ModelType type;
		P pos;
		float angle;// Unit: radians

		int recog = -1;// SSQK-Project just for temp usage!
	};

	struct LineArg {
		LineTag tag;
		LineEq eq;
		LineStyle style;
		CColor color = Config::Line_DefColor;
		int reserve = 0;
	};

	struct Status {
	    sq::ACC acc = sq::ACC::INVALID;
	    sq::FCW fcw = sq::FCW::INVALID;
	    sq::ALC alc = sq::ALC::INVALID;
	    sq::ACAS acas = sq::ACAS::INVALID;
		sq::NOA noa = sq::NOA::INVALID;
	};
}